#include "output_impl.h"
#include <base/fileUtils.h>
#include <base/geometry.h>
#include <base/utility.h>
#include <autosdk_hdmap/hdmap_layer_api.h>
#include <kalman_filter/particle_filter.h>
#include <iostream>
#include <fstream>

DbgFrameImpl::DbgFrameImpl(const std::string &img_path, const GeoPoint &gps_point, Camera *camera, double range) : camera_ptr_(camera)
{
	envelope2D env(gps_point.x, gps_point.y, gps_point.x, gps_point.y);
	env.ExpandBy(range);
	env.GetRange(min_, max_);

	double xrange = max_.x() - min_.x();
	double yrange = max_.y() - min_.y();
	col_ = camera_ptr_->width;
	row_ = camera_ptr_->height;
	xscale_ = col_ / xrange;
	yscale_ = row_ / yrange;

	combined_mat_ = cv::Mat(row_ * 2.0, col_, CV_8UC3, cv::Scalar(0, 0, 0));
	top_mat_ = combined_mat_(cv::Rect(0, 0, col_, row_));
	bottom_mat_ = combined_mat_(cv::Rect(0, row_, col_, row_));

	std::string image_file = FileUtils::file_concate(img_path, gps_point.imagename);
	cv::Mat origin_mat = cv::imread(image_file), undistort_mat;
	cv::undistort(origin_mat, undistort_mat, camera_ptr_->get_K(), camera_ptr_->get_D());
	undistort_mat.copyTo(top_mat_);
}

DbgFrameImpl::~DbgFrameImpl()
{
}

cv::Mat DbgFrameImpl::GetTopMat() const
{
	return top_mat_;
}

cv::Mat DbgFrameImpl::GetBottomMat() const
{
	return bottom_mat_;
}

Eigen::Vector2d DbgFrameImpl::toCanvas(double x, double y)
{
	double u = (x - min_.x()) * xscale_;
	double v = camera_ptr_->height - (y - min_.y()) * yscale_;
	return Eigen::Vector2d(u, v);
}

void DbgFrameImpl::DrawTrcPoints(const std::vector<GeoPoint> &gps_points, const cv::Scalar &color)
{
	if (gps_points.empty())
	{
		return;
	}

	for (std::size_t i = 0; i < gps_points.size() - 1; i++)
	{
		Eigen::Vector2d uv1 = toCanvas(gps_points[i].x, gps_points[i].y);
		Eigen::Vector2d uv2 = toCanvas(gps_points[i + 1].x, gps_points[i + 1].y);

		cv::line(bottom_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), color, 1.0);
		cv::circle(bottom_mat_, cv::Point(uv1.x(), uv1.y()), 2.0, color, 2.0);
	}
	Eigen::Vector2d uv3 = toCanvas(gps_points[gps_points.size() - 1].x, gps_points[gps_points.size() - 1].y);
	cv::circle(bottom_mat_, cv::Point(uv3.x(), uv3.y()), 2.0, color, 2.0);
}

void DbgFrameImpl::PrintIntoImg(const std::string &filename)
{
	cv::imwrite(filename, combined_mat_);
}

void DbgFrameImpl::DrawHDmapProjectView(const hdmap_frame &hdmap_f, const Pose &pose, const cv::Scalar &color)
{
	Eigen::Matrix3d bRw = pose.orientation.toRotationMatrix();
	Eigen::Vector3d bTw = pose.position;
	Eigen::Matrix3d cRb = camera_ptr_->mount.rotation.matrix();
	Eigen::Vector3d cTb = camera_ptr_->mount.translation;

	for (const hdmap_lane &lane : hdmap_f.hdmap_lanes)
	{
		for (std::size_t i = 0; i < lane.geometry.size() - 1; i++)
		{
			Eigen::Vector3d s = cRb.transpose() * (bRw.transpose() * (lane.geometry[i] - bTw) - cTb);
			Eigen::Vector3d e = cRb.transpose() * (bRw.transpose() * (lane.geometry[i + 1] - bTw) - cTb);

			Eigen::Vector2d uv1, uv2;
			if (Utility::Project3d_2d(s, uv1, camera_ptr_->K) && Utility::Project3d_2d(e, uv2, camera_ptr_->K))
			{
				cv::line(top_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), color, 2.0);
			}
		}
	}

	for (const hdmap_sign &sign : hdmap_f.hdmap_signs)
	{
		for (std::size_t i = 0; i < sign.geometry.size() - 1; i++)
		{
			Eigen::Vector3d s = cRb.transpose() * (bRw.transpose() * (sign.geometry[i] - bTw) - cTb);
			Eigen::Vector3d e = cRb.transpose() * (bRw.transpose() * (sign.geometry[i + 1] - bTw) - cTb);

			Eigen::Vector2d uv1, uv2;
			if (Utility::Project3d_2d(s, uv1, camera_ptr_->K) && Utility::Project3d_2d(e, uv2, camera_ptr_->K))
			{
				cv::line(top_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), color, 2.0);
			}
		}
	}
}

void DbgFrameImpl::DrawHorizontial(const GeoPoint &gps_point, const std::vector<hdmap_lane> &hdlanes)
{
	if (hdlanes.empty())
		return;

	for (std::size_t i = 0; i < hdlanes.size(); i++)
	{
		std::vector<Eigen::Vector3d> lane = hdlanes[i].geometry;
		if (!lane.empty())
		{
			for (std::size_t j = 0; j < lane.size() - 1; j++)
			{
				Eigen::Vector2d uv1 = toCanvas(lane[j].x(), lane[j].y());
				Eigen::Vector2d uv2 = toCanvas(lane[j + 1].x(), lane[j + 1].y());
				cv::line(bottom_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), cv::Scalar(255, 255, 255), 1.0);
			}
		}
	}
	Eigen::Vector3d center(gps_point.x, gps_point.y, gps_point.z);
	DrawArrow(center, gps_point.bearing, cv::Scalar(0, 0, 255));
}

void DbgFrameImpl::DrawArrow(const Eigen::Vector3d &center, double bearing, const cv::Scalar &color)
{
	Eigen::Vector2d uv1 = toCanvas(center.x(), center.y());
	cv::circle(bottom_mat_, cv::Point(uv1.x(), uv1.y()), 3.0, color, 3.0);

	Eigen::Vector3d dst = center + Eigen::Vector3d(2.0 * std::cos(bearing), 2.0 * std::sin(bearing), 0.0);
	Eigen::Vector2d uv2 = toCanvas(dst.x(), dst.y());
	cv::arrowedLine(bottom_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), color, 2.0);
}

void DbgFrameImpl::DrawParticles(Filtering::ParticleFilter *pf)
{
	std::vector<Filtering::Particle> particles = pf->particles;
	for (Filtering::Particle particle : particles)
	{
		Eigen::Vector2d uv1 = toCanvas(particle.x, particle.y);
		Eigen::Vector2d uv2 = toCanvas(particle.x + 1.0 * cos(particle.theta), particle.y + 1.0 * sin(particle.theta));
		cv::arrowedLine(bottom_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), cv::Scalar(0, 255, 0), 2.0);
	}
}

void DbgFrameImpl::DrawSolidLine3D(const std::vector<Eigen::Vector3d> &spline, const cv::Scalar &color, int line_width)
{
	if (spline.empty())
	{
		return;
	}
	for (std::size_t i = 0; i < spline.size() - 1; i++)
	{
		Eigen::Vector2d uv1 = toCanvas(spline[i].x(), spline[i].y());
		Eigen::Vector2d uv2 = toCanvas(spline[i + 1].x(), spline[i + 1].y());
		cv::line(bottom_mat_, cv::Point(uv1.x(), uv1.y()), cv::Point(uv2.x(), uv2.y()), color, line_width);
	}
}

void DbgFrameImpl::DrawRound2D(const Eigen::Vector2d &center, const cv::Scalar &color, double radius)
{
	cv::circle(top_mat_, cv::Point(center.x(), center.y()), radius, color, radius);
}

void DbgFrameImpl::DrawRectangle2D(const envelope2D &env, const cv::Scalar &color, double line_width)
{
	Eigen::Vector2d min_env, max_env;
	env.GetRange(min_env, max_env);

	std::vector<Eigen::Vector2d> geometry2D;
	geometry2D.emplace_back(Eigen::Vector2d(min_env.x(), min_env.y()));
	geometry2D.emplace_back(Eigen::Vector2d(max_env.x(), min_env.y()));
	geometry2D.emplace_back(Eigen::Vector2d(max_env.x(), max_env.y()));
	geometry2D.emplace_back(Eigen::Vector2d(min_env.x(), max_env.y()));
	geometry2D.emplace_back(Eigen::Vector2d(min_env.x(), min_env.y()));
	for (std::size_t i = 0; i < geometry2D.size() - 1; i++)
	{
		cv::line(top_mat_, cv::Point(geometry2D[i].x(), geometry2D[i].y()), cv::Point(geometry2D[i + 1].x(), geometry2D[i + 1].y()), color, line_width);
	}
}

void DbgFrameImpl::DrawSolidLine2D(const std::vector<Eigen::Vector2d> &spline, const cv::Scalar &color, int line_width)
{
	if (spline.empty())
	{
		return;
	}
	for (std::size_t i = 0; i < spline.size() - 1; i++)
	{
		cv::line(top_mat_, cv::Point(spline[i].x(), spline[i].y()), cv::Point(spline[i + 1].x(), spline[i + 1].y()), color, line_width);
	}
}

void DbgFrameImpl::DrawDlFrame2D(const DlFrame &dlFrame, const cv::Scalar &color)
{
	cv::Mat K = camera_ptr_->get_K(), D = camera_ptr_->get_D();
	std::vector<std::vector<cv::Point>> contours;
	std::vector<DlLane> lanes = dlFrame.lanes;
	for (DlLane lane : lanes)
	{
		std::vector<Eigen::Vector2d> undistort_points = lane.calibrated_polygon;
		if (!undistort_points.empty())
		{
			std::vector<cv::Point> ctor;
			for (std::size_t i = 0; i < undistort_points.size(); i++)
			{
				ctor.emplace_back(undistort_points[i].x(), undistort_points[i].y());
			}
			contours.push_back(ctor);
		}
	}

	std::vector<DlSign> signs = dlFrame.signs;
	for (DlSign sign : signs)
	{
		envelope2D calibrated_bbox = sign.calibrated_bbox;
		std::vector<cv::Point> ctor;
		ctor.emplace_back(calibrated_bbox.GetMinx(), calibrated_bbox.GetMiny());
		ctor.emplace_back(calibrated_bbox.GetMinx() + calibrated_bbox.Width(), calibrated_bbox.GetMiny());
		ctor.emplace_back(calibrated_bbox.GetMinx() + calibrated_bbox.Width(), calibrated_bbox.GetMiny() + calibrated_bbox.Height());
		ctor.emplace_back(calibrated_bbox.GetMinx(), calibrated_bbox.GetMiny() + calibrated_bbox.Height());
		contours.push_back(ctor);
	}
	cv::Mat black_mat(top_mat_.size(), CV_8UC3, cv::Scalar(0, 0, 0));
	cv::drawContours(black_mat, contours, -1, color, -1);
	cv::addWeighted(top_mat_, 0.7, black_mat, 0.3, 0.0, top_mat_);
}
